/*
 * regulator.c
 *
 *  Created on: 12/05/2011
 *      Author: morten
 */
//#include "BLDC.h"
#include "application.h"


volatile MOTOR_TypeDef Servo;



//volatile uint32_t periodServo = 1000000; //~48Hz PWM frequency, must match max ADC value
//TODO move to struct
#define SERVO_PERIOD	1000000 			//~48Hz PWM frequency, must match max ADC value
#define SERVO_MAX		980000		// 10% DC is full CCW
#define SERVO_MIN		900000				//SERVO_PERIOD * 0.9


/******************************************************************************
** Function name:
**
** Descriptions:
**
** parameters:
** Returned value:
**
******************************************************************************/
void initServo (MOTOR_TypeDef *ptr)
{

	ptr->max_mv = MOTOR_MAX_PERIOD;

	/****************************************************/
	/* Default PID structure initialization				*/
	/****************************************************/
	ptr->Kp = 5;
	ptr->Ki = 2;
	ptr->Kd = 0;
	ptr->sp = 800;	   	 // RPM
	ptr->pv = 0;
	ptr->IntError = 0;
	ptr->LastError = 0;
	ptr->mv = 0;
	ptr->HALstate[0] = 0;
	ptr->HALstate[1] = 0;
	ptr->TMRval[0] = 0;
	ptr->TMRval[1] = 0;
	ptr->Deadtime = 0;
	ptr->Direction = CW;
	ptr->CMT_step = 1;
	ptr->Startup = 35;
	ptr->max_mv = 1024;
	ptr->PolePairs = 1;//4;
	ptr->Brake = 0;
	ptr->Enable = 0;
	ptr->RPM = 0;
	ptr->RampingUp = FALSE;



	/* Initialize the PWM in timer32_1 enabling match0 output PIO0_1 */
	init_timer32PWM(0, SERVO_PERIOD, MATCH2);
	setMatch_timer32PWM(0, 2, SERVO_MIN);
	//enable_timer32(0);

	/* Throttle PWM */
	/* Initialize the PWM in timer32_1 enabling match0 output PIO1_7 */
	init_timer32PWM(0, SERVO_PERIOD, MATCH1);
	setMatch_timer32PWM(0, 1, SERVO_MIN);
	enable_timer32(0);

}

/*
 * Set value on servo engine
 * range 0-1024
 * */
void setServo(uint8_t device, uint32_t value)
{
	uint32_t dcVal;
	/*Futaba servo uses 0-10% dutycycle*/
	dcVal = ((((SERVO_MAX - SERVO_MIN) * value) >> 10) + SERVO_MIN);

	if (device == 1)
		setMatch_timer32PWM(0, 1, dcVal);
	else if (device == 2)
		setMatch_timer32PWM(0, 2, dcVal);

}
